package edu.wpi.first.wpilibj.swarm;
import com.sun.squawk.util.Arrays;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Utility;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

public class Arm implements Runnable {
	private DriverStation ds;
	private AnalogChannel pot;
	private double        target;
	private long          time;
	private double        dt;
	private long          tgttime;
	private double        offset;
	private double        vel;
	private double        pos;
	private double        filteredvel;
	private int           counter;
	private CANJaguar     motor;
	private double        output;
	private double[]      velpool;
	private double[]      velpoolsorted;
	private int           velptr;
	private boolean       enabled;
	private double        disabledout;
	
	private static final long   periodus = 20000;
	
	private static final double retract_voltage = 1.1;
	private static final double extend_voltage  = 3.96;
	
	private static final double  Kp             =  40.0;
	private static final double  Kd             =   1.0;
	private static final double  pwmlim         =   7.2;
	private static final int     Kvelfilter     =     4;
	private static final int     countertgt     =     3;
	private static final int     counterrst     =    -5;
	private static final double  stopvel        =  0.01;
	private static final double  nullzone       = 0.001;
	private static final double  pulsepwr       =   3.0;
	private static final double  Koffset        =   0.7;
	
	Arm() {
		init();
	}
	
	private void init() {
		new Thread(this).start();
	}
	
	synchronized void setTarget(double target) {
		this.target = target;
		if (target < 0.01) offset = 0.0;
		counter = counterrst;
	}
	
	private double getPos() {
		return (pot.getAverageVoltage() - retract_voltage) /
		         (extend_voltage - retract_voltage);
	}

	private double median(double[] values) {
		velpoolsorted = Arrays.copy(values, 0, Kvelfilter, 0, Kvelfilter);
		Arrays.sort(velpoolsorted);
		if (Kvelfilter % 2 == 0) {
			return (velpoolsorted[Kvelfilter/2-1] +
			        velpoolsorted[Kvelfilter/2])/2.0;
		} else {
			return velpoolsorted[Kvelfilter/2];
		}
	}
	
	private void calcState() {
		vel = (getPos() - pos) / dt;
		pos += vel*dt;
		velpool[velptr++] = vel;
		if (velptr >= Kvelfilter) velptr = 0;
		filteredvel = median(velpool);
	}
	
	private double limabsval(double val, double max) {
		if (val < -max) return -max;
		if (val >  max) return  max;
		return val;
	}
	
	private double sign(double val) {
		if (val > 0.0) return 1.0;
		if (val < 0.0) return -1.0;
		return 0.0;
	}
	
	private void pd() {
		output = limabsval(Kp * (target - pos) + offset - Kd * (filteredvel), pwmlim);
		if (target < 0.01 && output > 0.0) output = 0.0;
		try {
			motor.setX(-output);
		} catch (CANTimeoutException ex) {
			System.out.println(ex);
		}
	}
	
	private void pulse() {
		offset = limabsval(offset + Koffset * Kp * (target - pos), pwmlim);
		try {
			motor.setX(-limabsval(offset + sign(target - pos)*pulsepwr, pwmlim));
		} catch (CANTimeoutException ex) {
			System.out.println(ex);
		}
	}
	
	void setEnabled(boolean enable) {
		enabled = enable;
	}
	
	void setDisabledOut(double out) {
		disabledout = out;
	}
	
	synchronized private void control() {
		calcState();
		if (target > 0.01 &&
		    Math.abs(target - pos) > nullzone &&
		    Math.abs(filteredvel) < stopvel &&
		    ds.isEnabled()) {
			counter++;
			if (counter >= countertgt) {
				pulse();
				counter = counterrst;
			}
		} else if (counter > 0) counter--;
		pd();
	}

	public void run() {
		enabled       = false;
		disabledout   = 0.0;
		pot           = new AnalogChannel(4);
		pot.setOversampleBits(8);
		pot.setAverageBits(0);
		try {
			motor = new CANJaguar(2);
			motor.configMaxOutputVoltage(pwmlim);
			motor.configFaultTime(0.5);
			motor.changeControlMode(CANJaguar.ControlMode.kVoltage);
			motor.enableControl();
		} catch (CANTimeoutException ex) {
			System.out.println(ex);
		}
		target        = 0.0;
		offset        = 0.0;
		counter       = counterrst;
		vel           = 0.0;
		output        = 0.0;
		pos           = getPos();
		filteredvel   = vel;
		time          = Utility.getFPGATime();
		velpool       = new double[Kvelfilter];
		velpoolsorted = new double[Kvelfilter];
		velptr        = 0;
		ds            = DriverStation.getInstance();
		while(true) {
			dt       = ((double) (Utility.getFPGATime() - time))/1000000.0;
			time    += (long) (dt*1000000.0);
			tgttime += periodus;
			if (enabled)
				control();
			else {
				try {
					motor.setX(-disabledout);
				} catch (CANTimeoutException ex) {}
			}
			if (time >= tgttime) {
				//System.out.println("Loop too slow!");
				continue;
			}
			Timer.delay(((double)(tgttime - time))/1000000.0);
		}
	}
}